// #include "eigen/Eigen/Dense"
#include "eigen/Eigen/Core"
#include "eigen/Eigen/Geometry"
#include <cmath>
#include <iostream>

// Eigen::Quaterniond convert(Eigen::Matrix3d toBeConvertedM) {

// }

using namespace std;

int main() {

    Eigen::Matrix3d tbCM;
    tbCM << 0, 0, -1, -1, 0, 0, 0, 1, 0;
    // Eigen::Quaterniond(tbCM);
    Eigen::AngleAxisf rotation_vec_1(EIGEN_PI / 2, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rotation_vec_2(-EIGEN_PI / 2, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rotation_vec_3(EIGEN_PI / 2, Eigen::Vector3f::UnitZ());
    Eigen::Matrix3f tbCM_my(rotation_vec_1 * rotation_vec_2);
    // Eigen::Matrix3d tbCM_my(rotation_vec_3);
    cout.precision(3);
    cout << tbCM << endl;
    cout << tbCM_my.inverse() << endl;
    cout << rotation_vec_2.matrix().transpose() * rotation_vec_1.matrix().transpose()
         << endl;

    Eigen::AngleAxisd rotation_vector_z(
        EIGEN_PI / 4, Eigen::Vector3d(0, 0, 1)); //沿 Z 轴旋转 45 度
    Eigen::AngleAxisd rotation_vector_x(
        EIGEN_PI / 4, Eigen::Vector3d::UnitY()); //沿 Z 轴旋转 45
    cout << "rotation matrix =\n"
         << rotation_vector_z.matrix() << endl; //用matrix()转换成矩阵
    cout << "rotation matrix =\n"
         << rotation_vector_x.matrix() << endl; //用matrix()转换成矩阵

    return 0;
}